#include "System.h"
#include "GSLAM/core/GSLAM.h"

using namespace ORB_SLAM3;

int main(int argc,char** argv){

}

GSLAM::SE3 mat2se3(cv::Mat m){
    m.convertTo(m,CV_64F);
    GSLAM::SE3 T;
    T.fromMatrix((double*)m.data);
    return T;
}

REGISTER_SVAR_MODULE(orbslam3){
    sv::Class<System>("System")
            .construct<string,string,System::eSensor,bool>()
            .construct<sv::Svar>()
            .def("TrackMonocular",&System::TrackMonocular)
            .def("TrackStereo",&System::TrackStereo)
            .def("TrackRGBD",&System::TrackRGBD)
            .def("getMap",&System::getGMap);

    sv::Class<IMU::Point>("IMUPoint")
            .construct<double,double,double,double,double,double,double>()
            .def("__init__",[](sv::Svar v){
        if(v.isArray())
        {
            auto vec=v.castAs<std::vector<double>>();
            return IMU::Point(vec[0],vec[1],vec[2],vec[3],vec[4],vec[5],vec[6]);
        }
        else{
            return IMU::Point(v["acc"][0].castAs<double>(),
                    v["acc"][1].castAs<double>(),
                    v["acc"][2].castAs<double>(),
                    v["ang"][0].castAs<double>(),
                    v["ang"][1].castAs<double>(),
                    v["ang"][2].castAs<double>(),
                    v["timestamp"].castAs<double>());
        }
    })
    .def("json",[](IMU::Point& self)->sv::Svar{
        return {{"acc",{self.a.x,self.a.y,self.a.z}},
                {"ang",{self.w.x,self.w.y,self.w.z}},
                {"timestamp",self.t}};
    })
    ;

    sv::Class<cv::Mat>("Mat")
            .construct<>()
            .def("empty",&cv::Mat::empty)
            .def_readonly("data",&cv::Mat::data)
            .def("elemSize",&cv::Mat::elemSize)
            .def("elemSize1",&cv::Mat::elemSize1)
            .def("channels",&cv::Mat::channels)
            .def("type",&cv::Mat::type)
            .def("clone",&cv::Mat::clone)
            .def("__repr__",[](const cv::Mat& img){
                return to_string(img.cols)+"x"
                        +to_string(img.rows)+"x"+to_string(img.channels());})
            .def("__init__",[](sv::SvarBuffer buffer){
        if(buffer._holder.is<cv::Mat>())
            return buffer._holder.as<cv::Mat>();
        std::string format=buffer._format;
        std::vector<ssize_t> shape=buffer.shape;
        int channels=1;
        if(shape.size()==2)
            channels=1;
        else if(shape.size()==3)
            channels=shape[2];
        else
            LOG(FATAL)<<"Shape should be 2 or 3 demisson";

        int rows=shape[0];
        int cols=shape[1];

        static int lut[256]={0};
        lut['B']=0;lut['b']=1;
        lut['H']=2;lut['h']=3;
        lut['i']=4;lut['f']=5;
        lut['d']=6;
        int type=lut[format.front()]+((channels-1)<<3);
        return cv::Mat(rows,cols,type,(uchar*)buffer.ptr()).clone();
    })
    .def("__buffer__",[](cv::Mat& self){
        std::string formats[]={"B","b","H","h","i","f","d"};
        std::string format   =formats[(self.type()&0x7)];
        return sv::SvarBuffer(self.data,self.elemSize1(),format,{self.rows,self.cols,self.channels()},{},self);
    })
    ;

    sv::Class<Frame>("Frame")
            .def("keypoints",[](Frame& self){
        return self.mvKeys;
    })
            .def_readwrite("descriptors",&Frame::mDescriptors)
            .def_readwrite("Tcw",&Frame::mTcw)
            .def("depths",[](Frame& self){
        std::vector<double> depths;
        depths.reserve(self.mvpMapPoints.size());
        GSLAM::SE3 Tcw=mat2se3(self.mTcw);
        for(MapPoint* pt:self.mvpMapPoints){
            if(!pt)
            {
                depths.push_back(-1);
                continue;
            }
            auto p=pt->GetWorldPos2();
            GSLAM::Point3d pcam=Tcw*GSLAM::Point3d(p(0),p(1),p(2));
            depths.push_back(pcam.z);
        }
        return depths;
    })
    ;

    using namespace GSLAM;
    sv::Class<Point3d>("Point3d")
            .construct<>()
            .construct<double,double,double>()
            .def("dot",&Point3d::dot)
            .def("norm",&Point3d::norm)
            .def("at",&Point3d::at)
            .def("__repr__",&Point3d::toString)
            .def("__str__",&Point3d::toString)
            .def("__add__",&Point3d::add)
            .def("__sub__",&Point3d::sub)
            .def("__mul__",&Point3d::mul)
            .def("__div__",&Point3d::div)
            .def_readwrite("x", &Point3d::x)
            .def_readwrite("y", &Point3d::y)
            .def_readwrite("z", &Point3d::z);

    sv::Class<SO3>("SO3")
            .construct<>()
            .construct<double,double,double,double>()
            .construct<const Point3d&,double>()
            .construct<const double*>()
            .def("log",&SO3::log)
//            .def_static("exp",&SO3::exp<double>)
//            .def_static("fromPitchYawRollAngle",&SO3::fromPitchYawRollAngle)
            .def("normalise",&SO3::normalise)
//            .def("getMatrix",(Matrix<double,3,3> (SO3::*)()const)&SO3::getMatrix)
            .def("__mul__",&SO3::mul)
            .def("trans",&SO3::trans)
            .def("inverse",&SO3::inv)
            .def("__repr__",&SO3::toString)
            .def_readwrite("x", &SO3::x)
            .def_readwrite("y", &SO3::y)
            .def_readwrite("z", &SO3::z)
            .def_readwrite("w", &SO3::w)
            ;

    sv::Class<SE3>("SE3")
            .construct<>()
            .construct<const SO3&,const Point3d&>()
            .def("__init__",[](std::vector<double> v){
        if(v.size()!=7)
            LOG(FATAL)<<"SE3 need 7 parameters";
        return SE3(v[0],v[1],v[2],v[3],v[4],v[5],v[6]);
    })
            .def("log",&SE3::log)
            .def("inverse",&SE3::inverse)
            .def_static("exp",&SE3::exp<double>)
            .def("__mul__",&SE3::mul)
            .def("trans",&SE3::trans)
            .def("toString",&SE3::toString)
            .def("__repr__",&SE3::toString)
            .def_property("translation", &SE3::getTranslation, &SE3::setTranslation)
            .def_property("rotation", &SE3::getRotation, &SE3::setRotation)
            .def("tovec",[](SE3& s){
        auto t=s.get_translation();
        auto r=s.get_rotation();
        return std::vector<double>({t.x,t.y,t.z,r.x,r.y,r.z,r.w});
    });
            ;

    jvar["MONOCULAR"]=System::MONOCULAR;
    jvar["STEREO"]=System::STEREO;
    jvar["RGBD"]=System::RGBD;
    jvar["IMU_MONOCULAR"]=System::IMU_MONOCULAR;
    jvar["IMU_STEREO"]=System::IMU_STEREO;
}

EXPORT_SVAR_INSTANCE
